#include "Auto_Machine.h"

extern Motors *motor;
int T_flag;

void Auto_Machine(uint8_t buf_1, uint8_t buf_2)
{
    switch (buf_1)
    {
    case 'U':
        if (!T_flag)
        {
            switch (buf_2)
            {
            case '1':
                U1_Restore();
                break;
            case '\'':
                UC_Restore();
                break;
            case '2':
                U2_Restore();
                break;
            default:
                break;
            }
        }
        break;
    case 'D':
        if (!T_flag)
        {
            switch (buf_2)
            {
            case '1':
                D1_Restore();
                break;
            case '\'':
                DC_Restore();
                break;
            case '2':
                D2_Restore();
                break;
            default:
                break;
            }
        }
        break;
    case 'F':
        if (!T_flag)
        {
            switch (buf_2)
            {
            case '1':
                F1_Restore();
                break;
            case '\'':
                FC_Restore();
                break;
            case '2':
                F2_Restore();
                break;
            default:
                break;
            }
        }
        break;
    case 'B':
        if (!T_flag)
        {
            switch (buf_2)
            {
            case '1':
                B1_Restore();
                break;
            case '\'':
                BC_Restore();
                break;
            case '2':
                B2_Restore();
                break;
            default:
                break;
            }
        }
        break;
    case 'R':
        if (!T_flag)
        {
            switch (buf_2)
            {
            case '1':
                R1_Restore();
                break;
            case '\'':
                RC_Restore();
                break;
            case '2':
                R2_Restore();
                break;
            default:
                break;
            }
        }
        break;
    case 'L':
        if (!T_flag)
        {
            switch (buf_2)
            {
            case '1':
                L1_Restore();
                break;
            case '\'':
                LC_Restore();
                break;
            case '2':
                L2_Restore();
                break;
            default:
                break;
            }
        }
        break;
    case 'W':
        if (T_flag)
        {
            switch (buf_2)
            {
            case '1':
                Rotate_Cube();
                break;
            case '\'':
                CounterRotate_Cube();
                break;
            default:
                break;
            }
        }
        break;
    case 'T':
        switch (buf_2)
        {
        case '1':
           // if (!T_flag)
                Lift_Cube();
            break;
        case '\'':
           /// if (T_flag)
                CounterLift_Cube();
            break;
        default:
            break;
        }
        break;
    default:
        break;
    }
}

void Cube_Machine()
{
    int i = 0;
    if (UART1_Rx_flg)
    {
        for (i = 0; i < UART1_Rx_cnt / 2; i++)
            Auto_Machine(UART1_Rx_Buf[2 * i], UART1_Rx_Buf[2 * i + 1]);
        for (i = 0; i < UART1_Rx_cnt; i++)
            UART1_Rx_Buf[i] = 0;
        UART1_Rx_cnt = 0;
        UART1_Rx_flg = 0;
    }
}
void Cube_Machine_Init()
{
    HAL_Delay(100);

    T_flag = 0;

    motor->Down.Pull_GPIO = GPIOA;
    motor->Down.Pull_GPIO_PIN = GPIO_PIN_6;
    motor->Down.Direct_GPIO = GPIOA;
    motor->Down.Direct_GPIO_PIN = GPIO_PIN_7;

    motor->Back.Pull_GPIO = GPIOE;
    motor->Back.Pull_GPIO_PIN = GPIO_PIN_5;
    motor->Back.Direct_GPIO = GPIOE;
    motor->Back.Direct_GPIO_PIN = GPIO_PIN_6;

    motor->Face.Pull_GPIO = GPIOB;
    motor->Face.Pull_GPIO_PIN = GPIO_PIN_6;
    motor->Face.Direct_GPIO = GPIOB;
    motor->Face.Direct_GPIO_PIN = GPIO_PIN_7;

    motor->Left.Pull_GPIO = GPIOA;
    motor->Left.Pull_GPIO_PIN = GPIO_PIN_4;
    motor->Left.Direct_GPIO = GPIOA;
    motor->Left.Direct_GPIO_PIN = GPIO_PIN_5;

    motor->Right.Pull_GPIO = GPIOA;
    motor->Right.Pull_GPIO_PIN = GPIO_PIN_4;
    motor->Right.Direct_GPIO = GPIOA;
    motor->Right.Direct_GPIO_PIN = GPIO_PIN_5;

    motor->Lift.Pull_GPIO = GPIOA;
    motor->Lift.Pull_GPIO_PIN = GPIO_PIN_4;
    motor->Lift.Direct_GPIO = GPIOA;
    motor->Lift.Direct_GPIO_PIN = GPIO_PIN_5;
}
